// by mircemk March, 2026

#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <ESP32Servo.h>
#include "esp_pm.h"      
#include "esp_wifi.h"    

// --- КОНФИГУРАЦИЈА ПИНОВИ ---
const int PIN_SERVO   = 18; 
const int PIN_HALL    = 19; 
const int PIN_CURRENT = 4;   
const int PIN_BATTERY = 35;   
const int PIN_MOSFET  = 2; // Контрола на напојување за ACS712 и Servo

// --- ТАЈМЕРИ ---
unsigned long lastWatchdogTick = 0;
const unsigned long WATCHDOG_TIMEOUT = 300; 
unsigned long lastBatteryReport = 0;
const unsigned long BATTERY_REPORT_INTERVAL = 5000; 

// --- ПАРАМЕТРИ ---
RTC_DATA_ATTR int turnsNeeded = 1; 
int OFFSET = 5;     
const int S_U = 4;      
const int S_L = -4;     
const unsigned long IGNORE_TIME      = 600;   
const unsigned long SOFT_START_TIME  = 400;   
const unsigned long BRAKE_PULSE      = 25000; 
const int CURRENT_LIMIT              = 920;  
const int STALL_SAMPLES              = 3;    
float vZero                          = 2.5;

#define SERVICE_UUID           "12345678-1234-1234-1234-1234567890ab"
#define COMMAND_CHAR_UUID      "abcd1234-5678-1234-5678-1234567890ab"
#define STATUS_CHAR_UUID       "dcba4321-8765-4321-8765-1234567890ab"

Servo myServo;
BLECharacteristic *pStatusCharacteristic;
bool deviceConnected = false;
bool lastConnectionState = false;
volatile bool magnetHit = false;
bool hallEnabled = false;       
bool isManualMode = false;      
unsigned long moveStartTime = 0;
int lastDir = 0; 
int stallCounter = 0; 
int magnetCount = 0; 
String lockStatus = "LOCKED";
String lastKnownValidStatus = "LOCKED";

// --- ПОМОШНИ ФУНКЦИИ ---

void IRAM_ATTR onHall() { 
  if (hallEnabled && !isManualMode) magnetHit = true; 
}

void updateStatus(String newStatus) {
  lockStatus = newStatus;
  if (newStatus == "LOCKED" || newStatus == "UNLOCKED") lastKnownValidStatus = newStatus;
  pStatusCharacteristic->setValue(lockStatus.c_str());
  pStatusCharacteristic->notify(); 
}

void stopAction(String finalStatus) {
  int tempDir = lastDir;
  lastDir = 0; moveStartTime = 0; hallEnabled = false; stallCounter = 0; magnetCount = 0;
  
  if (tempDir == 1) myServo.write(S_L + 90 + OFFSET);
  else if (tempDir == -1) myServo.write(S_U + 90 + OFFSET);
  
  if (tempDir != 0) ets_delay_us(BRAKE_PULSE); 
  myServo.write(90 + OFFSET);

  // ИСКЛУЧИ ПЕРИФЕРИЈА (Штедење енергија)
  digitalWrite(PIN_MOSFET, LOW); 
  
  updateStatus(finalStatus);
}

float readBatteryVoltage() {
  long sum = 0;
  const int samples = 40;
  for (int i = 0; i < samples; i++) {
    sum += analogRead(PIN_BATTERY);
    delayMicroseconds(400);
  }
  float adcRaw = (float)sum / (float)samples;
  float vAdc = (adcRaw / 4095.0f) * 3.3f;
  float vBat = vAdc * 3.2f * 1.1f;
  return vBat;
}

void reportBatteryVoltage() {
  float vb = readBatteryVoltage();
  if (deviceConnected) {
    String msg = "BAT:" + String(vb, 2) + "V";
    pStatusCharacteristic->setValue(msg.c_str());
    pStatusCharacteristic->notify();
  }
}

float readCurrent() {
  long sum = 0;
  for (int i = 0; i < 15; i++) sum += analogRead(PIN_CURRENT);
  float voltage = ((float)sum / 15.0f * 3.3f) / 4095.0f;
  float current = (voltage - vZero) / 0.185f;
  return abs(current * 1000.0f);
}

void handleCommand(char cmd) {
  // Пред било која акција, ВКЛУЧИ напојување за мотор и сензор
  if (cmd == 'U' || cmd == 'L' || cmd == '[' || cmd == ']' || cmd == 'M') {
    digitalWrite(PIN_MOSFET, HIGH);
    delay(20); // Пауза за стабилизација на напонот
  }

  if (isManualMode) {
    if (cmd == '[' || cmd == ']') {
      lastWatchdogTick = millis();
      if (cmd == '[') { lastDir = 1; myServo.write(S_U + 90 + OFFSET); }
      else { lastDir = -1; myServo.write(S_L + 90 + OFFSET); }
      return;
    }
    if (cmd == '1') { turnsNeeded = 1; updateStatus("SET_1_TURN"); }
    else if (cmd == '2') { turnsNeeded = 2; updateStatus("SET_2_TURNS"); }
    else if (cmd == 'S') { stopAction("MAN_STOP"); }
    if (cmd == '1' || cmd == '2' || cmd == 'S') { delay(500); updateStatus(lastKnownValidStatus); }
  }

  if (cmd == 'M') { 
    isManualMode = !isManualMode;
    stopAction(isManualMode ? "MANUAL_ON" : "MANUAL_OFF");
    if (!isManualMode) { delay(500); updateStatus(lastKnownValidStatus); }
  }
  else if (!isManualMode) {
    if (cmd == 'U' || cmd == 'L') {
      magnetHit = false; hallEnabled = false; stallCounter = 0; magnetCount = 0;
      moveStartTime = millis(); 
      if (cmd == 'U') { lastDir = 1; myServo.write(S_U + 90 + OFFSET); updateStatus("UNLOCKING"); }
      else { lastDir = -1; myServo.write(S_L + 90 + OFFSET); updateStatus("LOCKING"); }
    }
    else if (cmd == 'S') { stopAction(lockStatus); }
  }
}

class MyServerCallbacks: public BLEServerCallbacks {
  void onConnect(BLEServer* pServer) { deviceConnected = true; }
  void onDisconnect(BLEServer* pServer) { deviceConnected = false; BLEDevice::startAdvertising(); }
};

class CommandCallbacks: public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic *pCharacteristic) {
    std::string rxValue = pCharacteristic->getValue();
    if (rxValue.length() > 0) handleCommand(rxValue[0]);
  }
};

void setup() {
  // 1. ЕНЕРГЕТСКА ОПТИМИЗАЦИЈА (Автоматски Light Sleep помеѓу BLE настани)
  esp_wifi_stop(); 
  esp_pm_config_esp32_t pm_config;
  pm_config.max_freq_mhz = 80;    
  pm_config.min_freq_mhz = 10;    
  pm_config.light_sleep_enable = true; 
  esp_pm_configure(&pm_config);

  Serial.begin(115200);

  // 2. MOSFET SETUP (Главен прекинувач)
  pinMode(PIN_MOSFET, OUTPUT);
  digitalWrite(PIN_MOSFET, LOW); // Почни со исклучена периферија

  // 3. ХАРДВЕР
  ESP32PWM::allocateTimer(0);
  myServo.setPeriodHertz(50);
  myServo.attach(PIN_SERVO, 500, 2400);
  pinMode(PIN_HALL, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(PIN_HALL), onHall, FALLING);
  pinMode(PIN_BATTERY, INPUT);
  analogReadResolution(12);

  // 4. BLE SETUP
  esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT);
  BLEDevice::init("BLE_LOCK_TEST");
  BLEServer *pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());
  BLEService *pService = pServer->createService(SERVICE_UUID);
  
  BLECharacteristic *pCmdChar = pService->createCharacteristic(COMMAND_CHAR_UUID, BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_READ);
  pCmdChar->setCallbacks(new CommandCallbacks());
  
  pStatusCharacteristic = pService->createCharacteristic(STATUS_CHAR_UUID, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
  pStatusCharacteristic->addDescriptor(new BLE2902());
  pStatusCharacteristic->setValue(lockStatus.c_str());
  
  pService->start();
  BLEDevice::getAdvertising()->start();
  
  myServo.write(90 + OFFSET);
  Serial.println("System v1.7 Ready - Peripherals OFF");
}

void loop() {
  if (Serial.available() > 0) handleCommand(Serial.read());

  if (deviceConnected && !lastConnectionState) {
    updateStatus(lockStatus);
  }
  lastConnectionState = deviceConnected;

  if (millis() - lastBatteryReport >= BATTERY_REPORT_INTERVAL) {
    lastBatteryReport = millis();
    reportBatteryVoltage();
  }

  if (isManualMode && lastDir != 0) {
    if (millis() - lastWatchdogTick > WATCHDOG_TIMEOUT) stopAction(lastKnownValidStatus);
  }

  if (lastDir != 0 && !isManualMode) {
    if (millis() - moveStartTime > SOFT_START_TIME) {
      float current = readCurrent();
      if (current > CURRENT_LIMIT) {
        stallCounter++;
        if (stallCounter >= STALL_SAMPLES) stopAction("Z");
      } else { if (stallCounter > 0) stallCounter--; }
    }
    if (magnetHit) {
      magnetCount++;
      if (magnetCount >= turnsNeeded) stopAction(lastDir == 1 ? "UNLOCKED" : "LOCKED");
      else { magnetHit = false; hallEnabled = false; moveStartTime = millis(); }
    }
    if (!hallEnabled && moveStartTime != 0 && (millis() - moveStartTime > IGNORE_TIME)) hallEnabled = true;
  }

  delay(10); 
}